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An Asymptotically-Optimal Sampling-Based Algorithm 
for Bi-directional Motion Planning 
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Abstract —Bi-directional search is a widely used strategy to 
increase the success and convergence rates of sampling-based 
motion planning algorithms. Yet, few results are available that 
merge both bi-directional search and asymptotic optimality into 
existing optimal planners, such as PRM*, RRT*, and FMT*. 
The objective of this paper is to fill this gap. Specifically, this 
paper presents a bi-directional, sampling-based, asymptotically- 
optimal algorithm named Bi-directional FMT* (BFMT*) that 
extends the Fast Marching Tree (FMT*) algorithm to bi¬ 
directional search while preserving its key properties, chiefiy 
lazy search and asymptotic optimality through convergence 
in probability. BFMT* performs a two-source, lazy dynamic 
programming recursion over a set of randomly-drawn samples, 
correspondingly generating two search trees: one in cost-to- 
come space from the initial configuration and another in cost- 
to-go space from the goal configuration. Numerical experiments 
illustrate the advantages of BFMT* over its unidirectional 
counterpart, as well as a number of other state-of-the-art 
planners. 

I Introduction 

Motion planning is the computation of paths that guide 
systems from an initial configuration to a set of goal configu¬ 
ration (s) around nearby obstacles, while possibly optimizing 
an objective function. The problem has a long and rich history 
in the field of robotics, and many algorithmic tools have been 
developed; we refer the interested reader to [1] and references 
therein. Arguably, sampling-based algorithms are among the 
most pervasive, widespread planners available in robotics, 
including the Probabilistic Roadmap algorithm (PRM) [2], 
the Expansive Space Trees algorithm (EST) [3], [4], and the 
Rapidly-Exploring Random Tree algorithm (RRT) [5]. Since 
their development, efforts to improve the “quality” of paths 
led to asymptotically-optimal (AO) variants of RRT and PRM, 
named RRT* and PRM*, respectively, whereby the cost of 
the returned solution converges almost surely to the optimum 
as the number of samples approaches infinity [6], [7]. Many 
other planners followed, including BIT* [8] and RRT^ [9] to 
name a few. Recently, a conceptually different asymptotically- 
optimal, sampling-based motion planning algorithm, called 
the Past Marching Tree (PMT*) algorithm, has been presented 
in [10], [11]. Numerical experiments suggested that PMT* 
converges to an optimal solution faster than PRM* or RRT*, 
especially in high-dimensional configuration spaces and in 
scenarios where collision-checking is expensive. 
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It is a well-known fact that bi-directional search can 
dramatically increase the convergence rate of planning 
algorithms, prompting some authors [12] to advocate its use 
for accelerating essentially any motion planning query. This 
was first rigorously studied in [13] and later investigated, for 
example, in [14], [15]. Collectively, the algorithms presented 
in [12]-[15] belong to the family of non-sampling-based 
approaches and are more or less closely related to a bi¬ 
directional implementation of the Dijkstra Method. More re¬ 
cently, and not surprisingly in light of these performance gains, 
bi-directional search has been merged with the sampling-based 
approach, with RRT-Connect and SBL representing the most 
notable examples [16], [17]. 

Though such bi-directional versions of RRT and PRM 
are probabilistically complete, they do not enjoy optimality 
guarantees. The next logical step in the quest for fast 
planning algorithms is the design of bi-directional, sampling- 
based, asymptotically-optimal algorithms. To the best of our 
knowledge, the only available results in this context are [18] 
and the unpublished work [19], both of which discuss bi¬ 
directional implementations of RRT*. Neither work, however, 
provides a mathematically-rigorous proof of asymptotic 
optimality starting from first principles. Accordingly, the 
objective of this paper is to propose and rigorously analyze 
such an algorithm. 

Statement of Contributions: This paper introduces the Bi¬ 
directional East Marching Tree (BEMT*) algorithmj^ To 
the best of the authors’ knowledge, this is the first tree- 
based, asymptotically-optimal bi-directional sampling-based 
planner. BEMT* extends EMT* to bi-directional search 
and essentially performs a “lazy,” bi-directional dynamic 
programming recursion over a set of probabilistically-drawn 
samples in the free configuration space. The contribution of 
this paper is threefold. Eirst, we present the BEMT* algorithm 
in Section |nl| Second, we rigorously prove the asymptotic 
optimality of BEMT* (under the notion of convergence 
in probability) and characterize its convergence rate in 
Section We note that the convergence rate of EMT* 
in [11] is proved only for obstacle-free configuration spaces, 
while we generalize that result to allow for the presence 
of obstacles. Einally, we perform numerical experiments in 
Section |V| across a number of planning spaces that suggest 
BEMT* converges to an optimal solution at least as fast as 
EMT*, PRM*, and RRT*, and sometimes significantly faster. 

II Problem Definition 

Let A’ be a d-dimensional configuration space, and let 
A’obs be the obstacle region, such that A' \ A’obs is an 

^The asterisk *, pronounced “star”, is intended to represent asymptotic 
optimality much like for the RRT* and PRM* algorithms. 


open set (we consider dX c A'obs)- Denote the obstacle- 
free space as A^free = cl(A’ \ A'obs), where cl(') denotes 
the closure of a set. A path planning problem, denoted 
by a triplet (Af^ee,Xinit,Xgoai), seeks to maneuver from 
an initial configuration xinit to a goal configuration Xgoai 
through Afree- Let a continuous function of bounded variation 
cr : [0, 1] ^ A', called a path, be collision-free if cr(r) G 
for all r G [0, 1]. A path is called di feasible solution to the 
planning problem (Afree, Xinit, Xgoai) if it is collision-free, 
(t( 0) = Xinit, and (t( 1) = Xgoai- 

Let S be the set of all paths. A cost function for the 
planning problem (Af^ee, xinit, Xgoai) is a function J : S ^ 
]R>o from H to the nonnegative real numbers; in this paper, 
we consider as J(cr) the arc length of a with respect to the 
Euclidean metric in A' (the extension to general cost functions 
will be briefly discussed in Section [IV-C| ). 

Optimal path planning problem: Given a path 
planning problem (Af^ee,xinit,Xgoai) and an arc 
length function J : S ^ M>o, find a feasible path 
cr* such that J(cr*) = min{ J(cr) | cr is feasible}. 

If no such path exists, report failure. 

Finally, we introduce some definitions concerning the 
clearance of a path, i.e., its “distance” from A’obs [HJ- 
a given > 0, the (i-interior of Afree is defined as the 
set of all points that are at least a distance 6 away from 
any point in A'obs- A collision-free path a is said to have 
strong (i-clearance if it lies entirely inside the (i-interior of 
Afree- A path planning problem with optimal path cost J* 
is called 6-robustly feasible if there exists a strictly positive 
sequence ^ 0, with 5n < d Mn ^ and a sequence 
{(7n}'^=i of feasible paths such that lim^r^oo 
and for all n G N, has strong -clearance, 0-^(1) = Xgoai, 
7^ Xgoai for all T G (0,1), and crn,(0) = Xiait- 


The last feature, which makes the algorithm “lazy,” may 
cause suboptimal connections. A central property of FMT* 
is that the cases where a suboptimal connection is made 
become vanishingly rare as the number of samples goes to 
infinity, which helps maintain the algorithm’s asymptotically 
optimality. This manifests itself into a key computational 
advantage—^by restricting collision detection to only locally- 
optimal connections, FMT* (as opposed to, e.g., PRM* [6]) 
avoids a large number of costly collision-check computations, 
at the price of a vanishingly small “degree” of suboptimality. 
We refer the reader to [10], [11] for a detailed description of 
the algorithm and its advantages. 

III-B BFMT* - High-level description 

At its core, BFMT* implements a bi-directional version 
of the FMT* algorithm by simultaneously propagating two 
wavefronts (henceforth, the leaves of an expanding tree will 
be referred to as the wavefront of the tree) through the free 
configuration space. BFMT*, therefore, performs a two-source 
dynamic programming recursion over a set of sampled points, 
and correspondingly generates a pair of search trees: one in 
cost-to-come space from the initial configuration and another 
in cost-to-go space from the goal configuration (see Fig. [^. 
Throughout the remainder of the paper, we refer to the former 
as the forward tree, and to the latter as the backward tree. 



Ill The BFMT* Algorithm 

In this section, we present the Bi-Directional Fast March¬ 
ing Tree algorithm, BFMT*, represented in pseudocode as 
Algorithm [T] To begin, we provide a high-level description of 
FMT* in Section |nl^ on which BFMT* is based. We follow 
in Section |III-B with BFMT* ’s own high-level description, 
and then provide additional details in Section |III-C| 


III-A FMT* - High-level description 

The FMT* algorithm, introduced in [10], [11], is a unidirec¬ 
tional algorithm that essentially performs a forward dynamic 
programming recursion over a set of sampled points and 
correspondingly generates a tree of paths that grow steadily 
outward in cost-to-come space. The recursion performed 
by FMT* is characterized by three key features: (1) It is 
tailored to disk-connected graphs, where two samples are 
considered neighbors (hence connectable) if their distance 
is below a given bound, referred to as the connection 
radius', (2) It performs graph construction and graph search 
concurrently', and (3) For the evaluation of the immediate cost 
in the dynamic programming recursion, one “lazily” ignores 
the presence of obstacles, and whenever a locally-optimal 
(assuming no obstacles) connection to a new sample intersects 
an obstacle, that sample is simply skipped and left for later 
(as opposed to looking for other locally-optimal connections 
in the neighborhood). 


Fig. 1: The BFMT* algorithm generates a pair of search trees: 
one in cost-to-come space from the initial configuration (blue) 
and another in cost-to-go space from the goal configuration 
(purple). The path found by the algorithm is in green color. 

The dynamic programming recursion performed by BFMT* 
is char acteriz ed by the same lazy feature of FMT* (see 
Section [Til- A| ). However, the time it takes to run BFMT* on a 
given number of samples can be substantially smaller than for 
FMT*. Indeed, for uncluttered configuration spaces, the search 
trees grow hyperspherically, and hence BFMT* only has to 
expand about half as far (in both trees) as FMT* in order to 
return a solution. This is made clear in Fig. [Ja), in which 
FMT* would have to expand the forward tree twice as far to 
find a solution. Since runtime scales approximately with edge 
number, which scales as the linear distance covered by the tree 
raised to the dimension of the state space, we may expect in 
loosely cluttered configuration spaces an approximate speed¬ 
up of a factor over FMT* in d-dimensional space (the 
— 1 in the exponent is because BFMT* has to expand 2 trees, 
so it loses one factor of 2 advantage). 

lU-C BFMT* - Detailed description 

To understand the BFMT* algorithm, some background 
notation must first be introduced. Let 5 be a set of points 
sampled independently and identically from the uniform 































distribution on Af^ee, to which xinit and Xgoai are added. 
(The extension to non-uniform sampling distributions is 


addressed in Section IV-C[ ) Let tree T be the quadruple 
(V, f, Vunvisited, Vopen), where V is the set of tree nodes, £ 
is the set of tree edges, and Vunvisited and Vopen are mutually 
exclusive sets containing the unvisited samples in S and 
the wavefront nodes in V, correspondingly. To be precise, 
the unvisited set Vunvisited stores all samples in the sample 
set S that have not yet been considered for addition to the 
tree of paths. The wavefront set Vopen, on the other hand, 
tracks in sorted order (by cost from the root) only those 
nodes which have already been added to the tree that are 
near enough to tree leaves to actually form better connections. 
These sets play the same role as their counterparts in FMT*, 
see [10], [11]. However, in this case BFMT* “grows” two 
such trees, referred to as T = (V, Vunvisited, Vopen) and 
V = (V', £', Kpen)- Initially, T is the tree rooted 

at Xinit, while T' is the tree rooted at Xgoai- Note, however, 
that the trees are exchanged during the execution of BFMT*, 
so T in Algorithmic is not always the tree that contains Xinit- 

The BFMT* algorithm is represented in Algorithm [C 
Before describing BFMT* in detail, we list briefly the 
basic planning functions employed by the algorithm. Let 
SAMPLEFREE(n) be a function that returns a set of n G 
N points sampled independently and identically from the 
uniform distribution on Afree- Let Cost(xx) be the cost of 
the straight-line path between configurations x and x. Let 
Path(z, T) return the unique path in tree T from its root to 
node z. Also, with a slight abuse of notation, let C0ST(x, T) 
return the cost of the unique path in tree T from its root to 
node X, and let CollisionFree(x, y) be a boolean function 
returning true if the straight-line path between configurations 
X and y is collision free. Given a set of samples A, let 
Near(^, z, r) return the subset of A within a ball of radius 
r centered at sample z (/.p., the set {x G ^ | ||x — z|| < r}). 
Let the Terminate function represent an external termination 
criterion {i.e., timeout, maximum number of samples, etc.) 
which can be used to force early termination (or prevent 
infinite runtime for infeasible problems). Finally, regarding 
tree expansion, let Swap(T, T') be a function that swaps 
the two trees T and T' . and let Companion(T) return the 
companion tree T' to T (or vice versa). 

We are now in position to describe the BFMT* algorithm. 
First, a set of n configurations in Af^ee is determined by draw¬ 
ing samples uniformly. Two trees are then initialized using the 
Initialize subfunction at the bottom of Algorithm [C with a 
forward tree rooted at Xinit and a reverse tree rooted at Xgoai. 
Once complete, tree expansion begins starting with tree T 
rooted at Xinit using the Expand procedure in Algorithm]^ 
In the following, the node selected for expansion will be 
consistently denoted by z, while Xmeet will denote the lowest- 
cost candidate node for tree connection (i.e., for joining the 
two trees). The Expand procedure requires the specification 
of a connection radius parameter, whose selection 
will be discussed in Section IV Expand implements the 
“lazy” dynamic programming recursion described (at a high 
level) in Section [Ill-B making locally-optimal collision-free 
connections from nodes x near z unvisited by tree T (those 
in set Vunvisited within search radius of z) to wavefront 
nodes x' near each x (those in set Vopen within search radius 
Tn of x). Any collision-free edges and newly-connected nodes 


found are then added to T, the connection candidate node 
Xmeet IS Updated, and z is dropped from the list of wavefront 
nodes. The key feature of the Expand function is that in the 
execution of the dynamic programming recursion it “lazily” 
ignores the presence of obstacles (see line [^ - as discussed 
in Section |IV| this comes at no loss of (asymptotic) optimality 
(see also [10], [11]). Note the Expand function is identical 
to that of unidirectional EMT*, with the exception here of 
additional lines for tracking connection candidate x^eet- 

After expansion, the algorithm checks whether a feasible 
path is found on line [7] If unsuccessful so far. Terminate 
(which reports failure upon early termination) is checked 
before proceeding. If the algorithm has not terminated, it 
checks whether the wavefront of the companion tree is empty 
(line [n]). If this is the case, the INSERT function shown in 
Algorithm samples a new configuration s uniformly from 
Afree and tries to connect it to a nearest neighbor in the 
companion tree within radius This way, the expanding 
tree is ensured to have at least one configuration in its 
wavefront available for expansion on subsequent iterations 
(the alternative would be to report failure). This mimics 
anytime behavior, and by forcing samples to lie close to tree 
nodes we effectively “reopen” closed nodes for expansion 
again. Uniform resampling may require many attempts before 
finding a configuration s which can be successfully connected 
to V'pgn, though this appeared to have a negligible impact 
on running time for our path planning studies. On the other 
hand, a more effective strategy might bias resampling towards 
areas requiring expansion {e.g., bottlenecks, traps) rather than 
uniformly within tree coverage. 

The algorithm then proceeds on lines 1^13 with the 
selection of the next node (and corresponding tree) for 
expansion. As shown, BEMT* “swaps” the forward and 
backward trees on each iteration, each being expanded in 
turns. As INSERT ensures the companion tree T' always has at 
least one node in its frontier V'pg^, a node is always available 
for subsequent expansion as the next z. After selection, the 
entire process is iterated. 

III-C.l BFMV - Variations: As for any bi-directional 
planner, the correctness and computational efficiency of 
BEMT* hinge upon two key aspects: (i) how computation 
is interleaved among the two trees (in other words, which 
wavefront at each step should be chosen for expansion), and 
(ii) when the algorithm should terminate. Eor instance, as 
an alternative tree expansion strategy (i.e., item (i)), one 
could replace lines [T2}|T3] with the “balanced trees” condition 
which enforces more of a balanced search, maintaining equal 
costs from the root within each wavefront such that the two 
wavefronts propagate and meet roughly equidistantly in cost- 
to-go from their roots: 

12: zi ^ argmin{C0ST(x, T)} 

t^open 

13: Z 2 ^ arg min{C0ST(x', T^)} 

^ ^t^open 

14: (z,T)^ argmin {Cost(z^, 7i)} 

(zi,r),(z2,r') 

15: T' = COMPANION(T) 

Similarly, as an alternative termination condition (i.e., item 
(ii)), one might replace line |7] with the “best path” criterion: 

7: ze (V'\V'pe„) 










Algorithm 1 The Bi-directional Fast Marching Tree 
Algorithm (BFMT*) 


Require: Query (xinit, Xgoai), Search radius rn, Sample count n 
1: <S ^ {xinit,Xgoal} U SAMPLEFREE(n) 

2: T ^ INITIALIZE(<S, Xinit) 

3: T' ^ lNITIALIZE(<S, Xgoal) 

4: Z i Xinit? Xmeet CT i 0 

5: while cr* = 0 

6l {XmeetjT} i EXPAND(7~, Z, Tn? Xmeet) 

21 if Xmeet 7“ ^ 

8: cr* ^ PATH(Xmeet, T) U PATH(Xmeet, T') 

9: break 

10: else if Terminate() then return Failure 

11: else if Vopen = ^ then T' ^ Insert(T', rn) 

12: argmin{C0ST(x', T^} 

13: SWAP(T,T') 

14: return a* 

1: function INITIALIZE(<S, xo) 

2: V ^ 0, ^ ^ 0, Vunvisited ^ Vopen <— 0 

3: return '7~ ADDN0DE((V, Vunvisited? Vopen)^ Xq) 


1 

function AddNode(T, x) 


2 

V ^ V U {x} 

> Add X to tree 

3 

8 ^ 8 U {(Xmin?x)} 

> Add edge to tree 

4 

Vunvisited ^ Vunvisited \ }x} 

> Mark x as visited 

5 

Vopen Vopen U }x} 

> Add X to wavefront 

6 

return T' (V, Vunvisited? Vopen) 


Currently line returns the first available path discovered, at 
the moment that the two wavefronts touch at Xmeet (which 
is not, in general, the lowest cost path). This alternative 
condition, on the other hand, returns the exact optimal path 
from Xinit to Xgoal through the given set 5 of n samples. 
This change terminates BFMT* when the two wavefronts 
have propagated sufficiently far through each other that no 
better solution can be discovered. Intuitively-speaking, this 
occurs at the first moment where the two trees have both 
selected, at the current iteration or previously, the same node 
as the minimum cost node z from their respective roots. 

Though seemingly promising ideas, no appreciable differ¬ 
ences in performance were found using the above criteria in 
combination or otherwise; hence we report only the simplest 
version of our planner as Algorithm 

IV Asymptotic Optimality of BFMT* 

In this section, we prove the asymptotic optimality of 
BFMT*. We begin with a result called probabilistic exhaus- 
tivity that essentially states that any path in Tf^ee may be 
“traced” arbitrarily well by connecting randomly-distributed 
points from a sufficiently large sample set covering Afree- We 
then prove the (asymptotic) optimality of BFMT* by showing 
that it returns solutions with costs no greater than that of 
any tracing path. The claim is proven assuming BFMT* acts 
without the INSERT procedure (Algorithm [^, in place of 
which “Failure” is reported instead. The proof for the full 
algorithm then follows immediately by a fortiori argument. 

IV-A Probabilistic exhaustivity 

Let cr : [0, 1] -A A' be a path. Given a set of samples 
(referred to as waypoints) C A', we associate a 

path ^ : [0, 1] ^ A' that sequentially connects the nodes 


Algorithm 2 Fast Marching Tree Expansion Step 
1: function EXPAND(T, Z, rn, Xmeet) 

2: Vopen,new 0 

3: -^near NEAR( Vunvisited? Z, Tn) 

4: for X C ^near 

5: Vnear NEAR( Vopen ? X, Tn) _ 

6: Xmin ^ argmin{C0ST(x, T) + Cost(xx)} 

Xnea.r 

7: if COLLISIONFREE(Xmin,x) 

8: (V, Vunvisited ? Vopen, new) 

ADDN0DE((V, Vunvisited? Vopen , new) ? x) 

9: if { X e V' and Cost(x, T) + Cost(x, T') < 

COST(Xnieet? 7~) T COST(Xnieet? "T ) } 

10: Xmeet ^x > Savc X as bcst conncction 

11- Vopen (Vopen U Vopen, new) \ {z} [> Add neW nodcS 

12: to the wavefront; drop z from the wavefront 

13: return "(Xmeet?T^ (V, Vunvisited? Vopen)} 


Algorithm 3 Insertion of New Samples 

1: function Insert(T, Vn) 

2: while Vopen = 0 and not Terminate() 

3: S ^ SaMPLEFREE(I) 

4: Vnear NEAR(V, S, T-u) 

5 : while Kear 7 ^ 0 

6: Xmin ^ argmin{C0ST(x, T) + Cost(^)} 

r^near 

7: if COLLISIONFREE(Xmin, s) 

8: T ^ ADDN0DE(T, s) 

9: break 

10: else then Vnear Vnear \ }Xmin} 

11: return 'T' i (V, Vunvisited? Vopen) 


yi: • • •: Ym with line segments. We consider the waypoints 
{Ym} to {e,r)-trace the path a if: (i) ||y,„ - y,„+i|| < r 
for all m, (ii) the cost of y is bounded as J{y) < (1 + e) J(cr), 
and (iii) the distance from any point of ^ to a is no more than 
r, i.e., min^^[o, 1 ]I|^(s) — a{t)\\ < r for all s G [0, 1]. In the 
context of sampling-based motion planning, we may expect to 
find closely-tracing {y^} as a subset of the sampled points, 
provided the sample size is large. This notion is formalized 
in the following theorem (Theorem |4.1| ), proved as Theorem 
IV.5 in [20] for the general case of driftless control-affine 
control systems, a special case of which is path planning 
without differential constraints (as addressed in this paper). 


Theorem 4.1 (Probabilistic exhaustivity): Define 
path planning problem (Af^ee? xinit, Xgoai) and let 
a : [0, 1] Afree be a feasible path. Denote the 

volume of the d-dimensional Euclidean unit ball by Q- 
Einally, let S = {xinit,Xgoai} U SAMPLEEREE(n), e > 0, 
and for fixed n consider the event An that there exist 
{ym}m=i c 5, 2/1 = Xinit, Vm = Xgoal which (e, r„) -trace 
cr, where 


r„ =4(l-f 7/); 


1 G V / M(AFree) “ Qogn 


V Cd 


( 1 ) 


for a parameter 77 > 0. Then, as n ^ oc, the probability 
that An does not occur (denoted by its complement A^) is 

asymptotically bounded as P[Vl^] = o(^n~^ log“^ n^. 






















IV-B Asymptotic optimality (AO) 

We are now in a position to prove the asymptotic optimality 
of BFMT*, which represents the main result of this section. 
We start with an important lemma, which relates the cost of 
the path returned by BFMT* to that of any feasible path. 

Lemma 4.2 (Bi-directional FMV cost comparison): Let 
cr : [ 0 , 1 ] ^ Tfree be a feasible path with strong (5-clearance. 
Consider running BFMT* to completion with n samples and 
a connection radius given by Eq. Q with 77 ^ 0. Let J^i 
denote the cost of the path returned by BFMT*. Then for 
fixed e > 0 : 

P[J„ > (1 + e)J(cr)] = o{n~^ log“3 ny 
Proof: Running BFMT* to completion generates one 
cost-to-come tree % and one cost-to-go tree Tg rooted at 
Xinit and Xgoai, respectively (subscripts i and g are used to 
resolve tree root ambiguity). If Xinit = Xgoai, then BFMT* 
immediately terminates with = 0, trivially satisfying the 
claim. Thus we assume that xinit 7 ^ Xgoai- Consider n suf¬ 
ficiently large so that < min{(5/2, e | |xinit — Xgoail | /2 }. 
Apply Theorem 14.1| to produce, with probability at least 
l — log“^ nj, a sequence of waypoints C 

S, Yi = Xinit, Ym = Xgoai which (e/2,rn)-trace a. We 
claim that in the event that such {y^} exists, the BFMT* 
algorithm returns a path with cost upper bounded as Jn < 
J{y)Frn < (1+e/2)J((7) + (e/2)J((7) = (l + e)J((7). 
The desired result follows directly. 

To prove the claim, assume the existence of an (e/2,rn)- 
tracing {Ym} of Lot represent a ball of radius r 

centered at a sample x. Note that our upper bound on 
implies that S(y^,rn) intersects no obstacles. This follows 
from our choice of and the distance bound 

inf ||y™-s||> inf ||cr„ - s|| - ||y„ - cr„|| 

St'^obs 

^ 2 rn, TYi ^ Tyi. 

where is the closest point of a to y^. This fact, along 
with ||y^ — Yni+ill ^ for all m, implies that when a 
connection is attempted for y^, both Ym-i Ym+i will 
be in the search radius and no obstacles will lie within that 
search radius. Running BFMT* to completion generates one 
cost-to-come tree Ti{Vi,£i, Vopen,i, Vunvisited,i) and one cost- 
to-go tree £g^ Voperijg', Vunvisited, 5 ^) rooted at Xj^it and 

Xgoai, respectively (the subscripts i and g are used to identify 
the root of a tree without ambiguity). The above discussion 
ensures that the trees will meet and the algorithm will return 
a feasible path when it terminates - the path outlined by the 
waypoints {y^} disallows the possibility of failure. 

For each sample point x G 5, let Ji(x) := Cost(x, 7^) 
denote the cost-to-come of x from Xinit in Ti, and let 
J^(x) := Cost(x, 7^) denote the cost-to-go from x to 
Xgoai in 7^. If X is not contained in a tree 77, k = {i^g}, 
we set J/c(x) = cx). When the algorithm terminates, we 
know there exists a sample point x^eet ^ Vi^Vg where 
the two trees meet; indeed we select the particular meeting 
point Xmeet = argmin^^y.py^ J^(x) + 7^(x). Then = 
Ji(xmeet) + -f^(xmeet)- We now note a lemma bounding the 
costs-to-come of the {y^}, the proof of which may be found 
as an inductive hypothesis (Eq. 5) in Theorem VI. 1 of [20]. 

Lemma 4.2: Let m G M}. If Ji{Ym) < 

then Ji(y„) < T,T=i\\yk - Vk+iW- Otherwise if y„ ^ 


Vi, then Ji(xi„eet) < “ Yfe+iH- Similarly if 

Jgiym) < 00, then Jg(y™) < | |yfc “ Yfe+i 11; oth¬ 

erwise Jg(Xmeet) < Efim I Wk “ Yk+l j I • 

To bound the performance 7^ of BFMT*, there are two 
cases to consider. Note in either case we find that < 
J(y) -f Vyi, thus completing the proof. 

Case 1 : There exists some y^ G Vi D . 

In this case, Jyi — Jiip^meet) Jgip^meet) ^ 

+ Jg{ym) < 00 hy our choice of x^eet- 
Then applying Lemma |4.3| we see that J„ < 

Ji{ym) + Jg^ym) < Ef=ll|yfe-yfe-till = 
^(y)- 


Case 2 : There are no y^ G Vi D V^. 

Consider fh = max{m | Ji{YYn) < oo}. Then 
Ym ^ L’i and can not have been the minimum 
cost element of Vopen,i at any point during algo¬ 
rithm execution or else we would have connected 
Ym+i ^ Li. Let z denote the minimum cost element 
of Vopen,i when Xmeet was added to Vi. We have 
the bound: 


^i(Xmeet) < '/i(z) + < Ji(y^^) -h r„ 

m—1 

k=l 


By our assumption for this case, y^ ^ Vg. 
Then by Lemma 4.3 we know that 7^(xmeet) < 
Eflmjlyfc -yfc+ilT- Combining with the pre- 
vious inequality yields = 7i(xmeet) + 

Jg(Xmeet) < Efllllyfc -yfc-KlII +rn = J{y) + 


Remark 4.4 (Tightened bound for connection radius): 

As discussed in [20], for the sake of clarity the constant 
term 4 in the expression for is greater than is necessary 
for Theorem |4.1| to hold. A more careful argument along the 
lines of the original FMT* AO proof [10] would suffice to 
show that a factor of 2 satisfies the theorem as well. 


Remark 4.5 (Alternative termination criteria): The proof 
holds as well for the different expansion and termination 
criteria discussed in Section |III-C.1| However, due to space 
constraints the details are omitted. 


We are now ready to show that BFMT* is asymptotically- 
optimal. The next theorem defines this formally. 

Theorem 4.6 (BFMT* asymptotic optimality): Assume a 
(5-robustly feasible path planning problem as defined in 
Section 1^ with optimal path a* of cost J*. Then BFMT* 
converges in probability to a* as the number of samples 
n ^ oo. Specifically, for any e > 0, 


lim P[Jn > (l + e)J*] =0 


Proof: The proof follows as a corollary to Lemma 4.2 


By our (5-robustly feasible assumption, we can find a strong 
(5-clearance feasible path a : [0, 1] ^ Tf^ee that approximates 
(7* with cost J((j) < (1 + e/3) J* (i.e., less than factor e/3 
from J*), for any e > 0. By Lemma 4.2 we can choose 
n sufficiently large such that BFMT* returns an e/3 cost 









V Simulations 


approximation to the approximant: 

J„ > (1 + e/3)V*] < P[J„ > (1 + e/3)J((T)] 

= log“^ 

To approach the optimal path, let the number of samples 
n ^ oo. It follows that, for any r] > 0\ 

Jn > (1 + e/3)^J* < lim o(n~d \og~d 77 ,) = 0 
J n^oo V / 

Now we relate this to the original claim. First suppose that 

e < 3. From (1 + e/3)^ < 1 + e, the event {> (1 + 

is a subset of the event |j^ > (1+ e/3)^J*|, hence: 


lim P 

n^oo 


Jn > {l+e/3fj* 


= 0 . 


lim P[J„ > (l + e)J*] < lim P 

n^oo n^oo 

Because the probability is monotone-decreasing in e as e 
increases, the statement holds for all e > 3 as well (to see this, 
apply Lemma |4^ again for m sufficiently large to handle e = 
3; then by similar argument as above P[Jm > (1 + e) J*] < 
P[-^m > (1 + 3) J*] = o(^m~d \og~d and take the 
limit as m ^ 00). Hence lim^^oo P[-^n > (1 + = 0 

holds for arbitrary e, and we see that BFMT* converges in 
probability to the optimal path, as claimed. ■ 

Remark 4.7 (Convergence rate): Note that we can also 
translate the convergence rate from Lemma 4.2 to the setup 
of Theorem |4.6[ which does not require strong (i-clearance. 
For any e > 0, the optimal path can be approximated by a 
strong-^-clear path with cost less than (1 + e) J(cr) and we can 
focus on approximating that path to high-enough precision 
to still approximate the optimal path to within (1 + e). Since 
the convergence rate in Lemma |4.2| only contains e in the 
rate’s constant, the big-0 convergence rate remains the same. 
This generalizes the convergence rate result in [11], which 
only applied to a specific obstacle-free configuration space, 
initial configuration, and goal region. 


IV-C Sampling and cost generalizations 

It is worth mentioning that the asymptotic optimality (AO) 
properties of BFMT* are not limited to uniform sampling 
and arc-length cost functions. For example, if one has prior 
information about areas that the optimal path is unlikely 
to pass through, it may be advantageous to consider a non- 
uniform sampling strategy that downsamples these regions. As 
long as the sampling density is lower-bounded by a positive 
number over the configuration space, BFMT* can be slightly 
altered (by merely increasing by a constant factor) to 
ensure it stays AO. The argument is analogous to that made 
in [11], and essentially proceeds by making the search radius 
wide enough to balance out the detrimental effect of the lower 
sampling density (in some areas). An additional common 
concern is when the cost is not arc-length, but some other 
metric or line integral cost. In either case, BFMT* need 
only consider cost balls instead of Euclidean balls when 
making connections. Details on adjusting the algorithm and 
why the AO proof still holds can be derived from [11]. The 
argument basically shows that the triangle inequality either 
holds exactly (for metric costs) or approximately, and that 
this approximation goes away in the limit as n ^ oc. 


In this section, we provide numerical path-planning exper¬ 
iments that compare the performance of BFMT* with other 
sampling-based, asymptotically-optimal planning algorithms 
(namely, FMT*, RRT*, and PRM*Q Given a planning 
workspace and query, we aim to observe the quality of the 
solution returned as a function of the execution time allotted 
to the algorithm. Here dynamic constraints are neglected 
and arc-length is used as path cost. As a basis for quality 
comparison between incremental or ’’anytime” planners (such 
as RRT*) and non-incremental planners (such as BFMT*, 
which generate solutions via sample batches), we vary the 
number of samples drawn by the planners during the planning 
process (which in essence serves as a proxy to execution time). 
Note sample count has a different connotation depending on 
the planner that will not necessarily be the number of nodes 
stored in the constructed solution graph - for RRT* (with one 
sample drawn per iteration), this is the number of iterations, 
while for FMT*, PRM*, and BFMT*, this is the number of 
free space samples taken during initialization. 


V-A Simulation Setup 


To generate simulation data for a given experiment, we 
queried the planning algorithms once each for a series of 
sample counts, recorded the cost of the solution returned, the 
planner execution tim^ and whether the planner succeeded 
or not, then repeated this process over 50 trials. To ensure a 
fair comparison, each planning algorithm was tested using 
the Open Motion Planning Library (OMPL) v 1.0.0 [21], 
which provides high-quality implementations of many state- 
of-the-art planners and a common framework for executing 
motion plans. In this way, we could ensure that all algorithms 
employed the exact same primitive routines (e.g., nearest- 
neighbor search, collision-checking, data handling, etc), and 
measure their performances fairly. Regarding implementation, 
BFMT*, FMT*, and PRM* used 7 ^ = 0 from Lemma [42 
for the nearest-neighbor radius in order to satisfy the 
theoretical bounds provided in Section IV and [6]. For RRT*, 
we used the default OMPL settings; namely, a 5% goal bias 
and a steering parameter equal to 20% of the maximum extent 
of the configuration space (except for the a-puzzle, in which 
case a value of 1.1 was found to work much better). For 
FMT*, we included the same INSERT routine as BFMT* 
for configuration resampling upon failure. For all algorithms, 
early termination (e.g., using Terminate for BFMT*) was 
suppressed by defining a 1000 second time limit, well above 
each planner’s worst-case execution time. 

Before proceeding, note that each marker shown on the 
plots throughout this section represents a single simulation 
at a fixed sample count. The points on the curves, however, 
represent the mean cost/time of successful algorithm runs only 
for a particular sample count, with error bars corresponding 


^Existing state-of-the-art sampling-based, bi-directional algorithms 
(namely, RRT-Connect and SBL) were initially also included. However, 
average costs for RRT-Connect and SBL were roughly 2-4x greater, which 
occluded the details of other curves; they were thus omitted for clarity 
^Code for all experiments was written in C++. Corresponding programs 
were compiled and run on a Linux-operated PC, clocked at 2.4 GHz and 
equipped with 7.5 GB of RAM. 












to one standard deviation of the 50 run sample mean|^ Sample 
counts varied from the order of 200 to 2000 points for 2D 
problems, from 1000 to 30000 points for 3D problems, and 
500 to 4000 points for the hypercube examples. 

V-B Results and Discussion 

Here we present benchmarking results (average solution 
cost versus average execution times and success rates) 
comparing BFMT* to other state-of-the-art sampling-based 
planners. Three benchmarking test scenarios were considered: 
(1) a 2D “bug trap” and (2) a 2D “maze” problem for a convex 
polyhedral robot in the §E(2) configuration space, as well as 
(3) a challenging 3D problem called the “a-puzzle” in which 
we seek to untangle two loops of metal (non-convex) in the 
§E(3) configuration space. All problems were drawn directly 
from OMPL’s bank of tests, and are illustrated in Fig. 

In each case, collision-checks relied on OMPL’s built-in 
collision-checking library, FCL. Additionally, to tease out the 
performance of BFMT* relative to FMT* in high-dimensional 
environments, we also studied a point mass robot moving in 
cluttered unit hypercubes of 5 and 10 dimensions 



(a) SE(2) bug trap (b) SE(2) maze (c) SE(3) o-puzzle 


Fig. 2: Depictions of the OMPL rigid-body planning problems 

Figureshows the results for each BFMT*, FMT*, RRT*, 
and PRM*. Performance here is measured by execution time 
on the x-axis and solution cost on the y-axis—high quality 
data points are therefore located in the lower-left corner 
(low-cost solutions obtained quickly). The plots reveal that 
both FMT* and BFMT* for the most part outperform RRT* 
as well as PRM*. In particular, BFMT* and FMT* achieve 
higher success rates (always a fiat 100% for the cases studied) 
in shorter time. To extract further information, we need to 
examine each test in detail. 

In the Bug Trap and Maze problems, BFMT* notably 
generates the same cost-time curve as FMT* (meaning they 
return solutions of very similar cost for a given sample 
count), but with data points shifted to the left (indicating they 
were obtained in shorter execution time). Though not shown 
due to slow running times for PRM* (whose results had to 
be truncated to clarify detail), all planners appear to tend 
towards similar low-cost solutions as more execution time 
was allocated. However BFMT* and FMT* seem to converge 
to an optimum much faster, particularly for the Maze problem 
(on the order of 1.5 and 2.0 seconds respectively, compared 
to 3-4 seconds for RRT* and 5-7 seconds for PRM*). This 
contrast becomes even more evident for the a-puzzle. Here 


^Standard deviation of the mean indicates where we expect with one-cr 
confidence the distribution mean to lie based on the 50-run sample mean, 
and is related to the standard deviation of the distribution by cr^ = cr/v^. 

^We populated the space to 50% obstacle coverage with randomly-sized, 
axis-oriented hyperrectangles. Xinit was set to the center at [0.5,..., 0.5], 
with the goal Xgoai at the ones-vector (i.e., 


we see an unusual spread of solutions - one in a band at 
around 500 cost and another at around 275. These indicate 
the presence of two solution types, or homotopy classes: 
one corresponding to the true ct-puzzle solution, and another 
less-efficient path. This appears to have yielded a “bump” in 
the BFMT* cost-curve, where increasing the sample count 
momentarily gives an increased average cost. We believe this 
is a result of how BFMT* trees interconnect; at this count, by 
unlucky circumstance, the longer homotopy seems to be found 
first more often than usual. But as proved in Section the 
behavior disappears as n ^ oo. Note RRT* seems to avoid 
this issue through goal biasing. Despite the difficult problem 
structure, BFMT* finds the cheaper homotopy faster than 
other planners, with many more of its data points clustered 
in the lower-left corner, generally at lower costs and times 
than RRT* and of equal quality but faster times than FMT*. 

These results suggest that BFMT* tends to an optimal cost 
at least as fast as the other planners, and sometimes much 
faster. To shed light on the relative performance of FMT* 
and BFMT* further, we compare them in higher dimensions. 
Results for the 5D and lOD hypercube are shown in Fig. 
(success rates were again at 100%, and were thus omitted). 
Here BFMT* substantially outperforms FMT*, particularly 
as dimension increases, with convergence in roughly 0.5 and 
1.4 seconds (5D), and 5 and 20 seconds (lOD) on average. 
This suggests that reachable volumes play a significant role in 
their execution time. The relatively small volume of reachable 
configurations around the goal at the corner implies that 
the reverse tree of BFMT* expands its wavefront through 
many fewer states than the forward tree of FMT* (which 
in fact needlessly expands towards the zero-vector); tree 
interconnection in the bi-directional case prevents its forward 
tree from growing too large compared to unidirectional search. 
This is pronounced exponentially as the dimension increases. 
In trap or maze-like scenarios, however, bi-directionality does 
not seem to change significantly the number of states explored 
by the marching trees, leading to comparable performance 
for the SE(2) bug-trap and maze. Note we expect a greater 
contrast in execution times in favor of BFMT* as the cost of 
collision-checking increases, such as with many non-convex 
obstacles or in time-varying environments. 

VI Conclusion 

In this paper, we presented a bi-directional, sampling-based, 
asymptotically-optimal motion planning algorithm named 
BFMT*, for which we rigorously proved its optimality and 
characterized its convergence rate - arguably firsts in the 
field of bi-directional sampling-based planning. Numerical 
experiments in §E(2), and §E(3) revealed that BFMT* 
tends to an optimal solution at least as fast as its state-of- 
the-art counterparts, and in some cases significantly faster. 
Convergence rates are expected to improve with paralleliza¬ 
tion, in which each tree is grown using a separate CPU. 

Future research will examine BFMT*’s interaction with 
more advanced techniques, such as adaptive sampling near 
narrow passages or sample biasing in INSERT (Algorithm 
towards failed wavefronts. We also plan to extend BFMT* to 
dynamic environments through lazy re-evaluation (leveraging 
its tree-like forward and reverse path structures) in a way that 
reuses previous results as much as possible. Maintaining 
bounds on run-time performance and solution quality in 
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Fig. 3: Simulation results for the three OMPL scenarios. 
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Fig. 4: FMT* and BFMT* results for 5D and lOD cluttered 
hypercubes (50% coverage; all success rates were 100%). 
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this new context will be the greatest challenges. Ultimately, 
we hope that BFMT* will enable fast, easy-to-implement 
planning and re-planning in a wide range of time-varying 
scenarios, much as we have shown here for the static case. 
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